module Control_module
  def movemotor(left,right)
    @connect.write("DRIVE {Left #{left}} {Right #{right}} \r\n")
  end
  
  #orders the robot to move forward 
  # Note:Only usable with the wheeled robots, P2AT etc.
  def moveforward
    Thread.new{
    Thread.current["name"]="r_#{@robotname}_moveforward"
    @connect.write("DRIVE {Left 1.0} {Right 1.0} \r\n")
    sleep(2)
    @connect.write("DRIVE {Left 0.0} {Right 0.0} \r\n")
    }
  end
  
  #orders the robot to move backwards
  # Note:Only usable with the wheeled robots, P2AT etc.
  def movebackward
    Thread.new{
    Thread.current["name"]="r_#{@robotname}_movebackward"
    @connect.write("DRIVE {Left -1.0} {Right -1.0} \r\n")
    sleep(2)
    @connect.write("DRIVE {Left 0.0} {Right 0.0} \r\n")
    }
  end
   
  #orders the robot to turn to the left 
  # Note:Only usable with the wheeled robots, P2AT etc.
  def moveleft
    Thread.new{
    Thread.current["name"]="r_#{@robotname}_moveleft"
    @connect.write("DRIVE {Left -1.0} {Right 1.0} \r\n")
    sleep(1)
    @connect.write("DRIVE {Left 0.0} {Right 0.0} \r\n")
    }
  end
  
  #orders the robot to turn to the right
  # Note:Only usable with the wheeled robots, P2AT etc.
  def moveright
    Thread.new{
    Thread.current["name"]="r_#{@robotname}_moveright"
    @connect.write("DRIVE {Left 1.0} {Right -1.0} \r\n")
    sleep(1)
    @connect.write("DRIVE {Left 0.0} {Right 0.0} \r\n")
    }
  end
    
  #this function accepts a Fixnum as an input, and than the robot
  #turns right for the inputed amount of radians  (Parameter=0.2)
  def smartright(angle)
    if @moveflag==""
#    @static_col=0 #used for static collision checks     
#    @static_col_back=0 #used for static collision checks
    
    beforeyaw=@ins[5].to_f #check the current yaw position
    
    #calculate the targeted yaw
    if(beforeyaw+angle<6.28)
      targetyaw=beforeyaw+angle
    else
      targetyaw=(angle+beforeyaw)-6.28
    end  
    puts beforeyaw,targetyaw
    @moveflag="right"
    
    #turn till targeted yaw
    while ((@ins[5].to_f-targetyaw).abs >0.2)
       @connect.write("DRIVE {Left 1.0} {Right -1.0} \r\n")
       sleep(0.05)
        if (@stop==1)
       break 
        end
    end   
    #break
       @connect.write("DRIVE {Left 0.0} {Right 0.0} \r\n")
       @moveflag="" 
    sleep(1.0) # stop for 1 seconds
    end
 end
  
  #this function accepts a Fixnum as an input, and than the robot
  #turns left for the inputed amount of radians (Parameter=0.2)
  def smartleft(angle)
    if @moveflag==""
      
#    @static_col=0 #used for static collision checks   
#    @static_col_back=0 #used for static collision checks
   
    beforeyaw=@ins[5].to_f #check the current yaw position
    #calculate the targeted yaw
    if(beforeyaw-angle>0)
      targetyaw=beforeyaw-angle
    else
      targetyaw=6.28-(angle-beforeyaw)
    end  
    @moveflag="left"
    
    #turn till targeted yaw
    while ((@ins[5].to_f-targetyaw).abs >0.2 )
       @connect.write("DRIVE {Left -1.0} {Right 1.0} \r\n")
       sleep(0.05)
       if (@stop==1)
       break 
        end
    end   
    #break
       #puts(@ins[5].to_f-targetyaw).abs 
       @connect.write("DRIVE {Left 0.0} {Right 0.0} \r\n")
       @moveflag="" 
    sleep(1.0) # stop for 1 seconds
    end
 end
  
 
  #orders the robot to stop by interrupting the smartfor commands
  #can only be used to stop movement by smartfor, smartback
  def smartbreak
    @stop=1
  end
  
  # normal smartmove forward without the accurate distance
    def smartfor(distance,speed)
     callsmartfor(distance,speed,0)
    end
  
  
  
  #Smart moves the robot forward, <distance> unreal Units.
  #in order for the robot to move forward accurately, the robot moves slowly 
  #has an error rate of about 0.01-0.02 %
    def smartfor_acc(distance,speed)
      callsmartfor(distance,speed,1)
    end
  
  
  
  #is the call function for the smartfor functions
  def callsmartfor(distance,speed,mode)
    #mode 0 is none acc
    #mode 1 is acc
   if @moveflag=="" #if there is no move order
    distance_f=distance
    @speed=speed
    movedis=0.0
    #read the sensor information
    xpos1=@ins[0].to_f
    ypos1=@ins[1].to_f
    zpos1=@ins[2].to_f
    #@connect.write("DRIVE {Left 1.0} {Right 1.0} \r\n")
    @connect.write("DRIVE {Left #{@speed}} {Right #{@speed}} \r\n")
    @reducedtime=0
    sleep(0.5) #give some time to start the engine, used with static_col
    while movedis< distance_f - (@P2ATPARA)
      #Set the motion status to moving
      @moveflag="forward"
      #check current position
      self.recieve
      xpos2=@ins[0].to_f
      ypos2=@ins[1].to_f
      zpos2=@ins[2].to_f
      movedis=Math.sqrt((xpos2-xpos1).abs*(xpos2-xpos1).abs+(ypos2-ypos1).abs*(ypos2-ypos1).abs+(zpos2-zpos1).abs*(zpos2-zpos1).abs )
      #check if no obsticles existes at the range of 0.5 meters
      #if (@fSONARSEN[3].to_f < 0.5 or @fSONARSEN[4].to_f < 0.5 )
      if (@stop==1 )
       break 
      end
   #if mode is 1, run the slow down
   if(mode==1)    
     #reduce speed if moved only 0.3 units is remaining 
     if(@reducedtime==0 and distance_f-movedis <0.3   )   
       @reducedtime=1
       @speed=1
       @connect.write("DRIVE {Left #{@speed}} {Right #{@speed}} \r\n")
     end
   end
          
  end
  @stop=0
  @moveflag="" 
  @connect.write("DRIVE {Left 0.0} {Right 0.0} \r\n") # order the robot to stop after normal movement
  #puts movedis
  #if accurate movement, stop at the end      
  if(mode==1)   
     sleep(1.0) # stop for 1 seconds
  end
        
    end #check flags
  end
  
  
 
  #Smart moves the robot forward, <distance> unreal Units.
  #in order for the robot to move forward accurately, the robot moves slowly 
  #has an error rate of about 0.01-0.02 %
  def smartback(distance,speed)
    if @moveflag==""
    distance_f=distance
    @speed=speed
    movedis=0.0
    #read the sensor information
    xpos1=@ins[0].to_f
    ypos1=@ins[1].to_f
    zpos1=@ins[2].to_f
    @connect.write("DRIVE {Left -#{@speed}} {Right -#{@speed}} \r\n")
    
    @reducedtime=0  
    while movedis<distance_f-@P2ATPARA
    
    #check current position
    self.recieve
    
    #Set the motion status to moving
    @moveflag="backward"
    xpos2=@ins[0].to_f
    ypos2=@ins[1].to_f
    zpos2=@ins[2].to_f
    movedis=Math.sqrt((xpos2-xpos1).abs*(xpos2-xpos1).abs+(ypos2-ypos1).abs*(ypos2-ypos1).abs+(zpos2-zpos1).abs*(zpos2-zpos1).abs )
    #check if no obsticles existes at the range of 0.5 meters
     if (@stop==1)
        break 
      end
        
    #reduce speed if moved only 0.3 units is remaining
     if(@reducedtime==0 and distance_f-movedis <0.3   )   
       @reducedtime=1
       @speed=1
       @connect.write("DRIVE {Left -#{@speed}} {Right -#{@speed}} \r\n")
     end  
        
    end
     @stop=0
     @connect.write("DRIVE {Left 0.0} {Right 0.0} \r\n")
      #Set the motion status to moving
      @moveflag="" 
      #  sleep(1.0) # stop for 1 seconds
    end #check flags
  end
     
  
end


